package com.hitqz.robot.driver.schedule;

import cn.hutool.core.net.NetUtil;
import com.google.common.base.Objects;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.util.DataTypeEnum;
import com.hitqz.robot.driver.util.KcCacheUtil;
import com.hitqz.robot.driver.util.KcToolkit;
import com.hitqz.robot.driver.util.SequenceUtil;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Component;

/**
 * 定时查询
 *
 * @author wengyx
 * @date 2024/09/02
 */
@Component
@Slf4j
public class QuerySchedule {

    private static boolean lastSpeedCtrl = false;

    /**
     * 定时轮询机器人运行状态
     * @throws InterruptedException
     */
    @Scheduled(fixedRate = 200)
    private void queryRunningStatus() throws InterruptedException {
        if (this.checkNet()) {
            NetworkServiceThreadPool.dealMessage(KcToolkit::queryRunningStatus);
        }
    }

    /**
     * 定时轮询机器人导航状态
     * @throws InterruptedException
     */
    @Scheduled(fixedRate = 200)
    private void queryNavStatus() throws InterruptedException {
        if (this.checkNet()) {
            NetworkServiceThreadPool.dealMessage(KcToolkit::queryNavStatus);
        }
    }


    /**
     * 轮询变量值
     */
    //@Scheduled(fixedRate = 2000)
    @Scheduled(fixedRate = 1000)
    private void queryVar(){
        if (this.checkNet()) {
            DataTypeEnum.VAR_NAME_SET.forEach(name->{
                try {
                    KcToolkit.readVar(name);
                }catch (Exception e){

                }
            });
        }
    }


    /**
     * 控制速度时要定时100ms发心跳，1s内没发的话，控制器的组态软件会强制速度置0
     * @throws InterruptedException
     */
    @Scheduled(fixedRate = 100)
    private void sendCmdSpeedHeartbeat() throws InterruptedException {
        NetworkServiceThreadPool.dealMessage(()->{
            if (this.checkNet() && KcCacheUtil.getSpeedFlag()) {
                KcToolkit.writeVar("cmd_heartbeat", DataTypeEnum.UDINT, String.valueOf(SequenceUtil.nextSpeedCmdHeartBeatId()), true);
            }
        });
    }

    /**
     * 切换控制模式-用于web遥控速度
     * @throws InterruptedException
     */
    @Scheduled(fixedRate = 250)
    private void changeCtrlMode() throws InterruptedException {
        boolean currentSpeedFlag = KcCacheUtil.getSpeedFlag();
        if (!Objects.equal(lastSpeedCtrl, currentSpeedFlag)){
            String currentMode = KcToolkit.readVar("cmd_forcedctrl");
            if (currentSpeedFlag) {
                // 切换上位机模式 写速度变量才能生效
                boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "1", true);
                log.info("[kc] 切换上位机模式{}",result);
            }else {
                // 速度置 0
                KcToolkit.writeVar("cmd_vel_line_x",DataTypeEnum.REAL,"0.0", false);
                KcToolkit.writeVar("cmd_vel_line_y",DataTypeEnum.REAL,"0.0", false);
                KcToolkit.writeVar("cmd_vel_angle",DataTypeEnum.REAL,"0.0",false);
                // 切换上位机模式
                boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "0",true);
                log.info("[kc] 切换自动模式 {}",result);
            }
            String changedMode = KcToolkit.readVar("cmd_forcedctrl");
            log.info("[kc] cmd_forcedctrl {} -> {}",currentMode,changedMode);
        }
        lastSpeedCtrl = currentSpeedFlag;
    }


    /**
     * 减少查询频率
     * @return
     */
    private boolean checkNet(){
        if (KcToolkit.client == null) {
            return false;
        }
        if (!NetUtil.ping(KcToolkit.kcControlIp,2000)){
            return false;
        }
        return KcToolkit.client.readyToSend();
    }


}
